#include "rclcpp/rclcpp.hpp"
#include "bs_complex/msg/complex_num.hpp"
#include "bs_norm/srv/norm.hpp"

#include <memory>

#include <math.h>

void norm(const std::shared_ptr<bs_norm::srv::Norm::Request> request,
          std::shared_ptr<bs_norm::srv::Norm::Response>      response)
{
  float re,im;
  re = request->complex.re;
  im = request->complex.im;
  response->norm = sqrtf32(re*re + im*im);
  
//   response->norm = sqrtf32((request->complex.re) + request->b);
  RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Incoming request re=%.3f im=%.3f", re, im);
  RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "sending back response: %.3f", response->norm);
}

int main(int argc, char **argv)
{
  rclcpp::init(argc, argv);

  std::shared_ptr<rclcpp::Node> node = rclcpp::Node::make_shared("norm_serve");

  rclcpp::Service<bs_norm::srv::Norm>::SharedPtr service = node->create_service<bs_norm::srv::Norm>("complex_norm", &norm);

  RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Ready to cal norm of Complex.");

  rclcpp::spin(node);
  rclcpp::shutdown();

}